#include "robot.h"

int main(void)
{
    frame *trelw;
    frame *srelb;
    frame *tmp;
    vec3 v1={0.1,0.2,30.0};
    vec3 v2={-0.1,0.3,0.0};
    vec3 theta={0.0,90.0,-90.0};
    
    trelw = UTOI(v1);
    srelb = UTOI(v2);
    
    tmp = WHERE(theta, trelw, srelb);
    print_frame(tmp, stdout);
    /* Reasignamos theta */
    theta[0] = -23.6;
    theta[1] = -30.3;
    theta[2] = 48.0;
    /* Liberamos tmp antes de asignarle nueva memoria para no perder memoria */
    gsl_matrix_free(tmp);
    tmp = WHERE(theta, trelw, srelb);
    print_frame(tmp, stdout);
    /* Otra vez los pasos anteriores */
    theta[0] = 130.0;
    theta[1] = 40.0;
    theta[2] = 12.0;
    gsl_matrix_free(tmp);
    tmp = WHERE(theta, trelw, srelb);
    print_frame(tmp, stdout);
    
    /* Antes de terminar liberamos la memoria */
    gsl_matrix_free(tmp);
    return 0;
}